#include "mainwindow.h"
#include "ui_mainwindow.h"
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <QDebug>
#include "calculatethreepointmatrixtransform.h"
#include <vector>
using namespace cv;
using namespace std;

MainWindow::MainWindow(QWidget *parent) :
    QMainWindow(parent),
    ui(new Ui::MainWindow)
{
    ui->setupUi(this);

    calibrate=new LaserCameraCalibrate;
    timer.start( 300);
    QObject::connect(&timer, SIGNAL( timeout() ), this, SLOT(timeOut_slot()));

}

MainWindow::~MainWindow()
{
    delete ui;
}

void MainWindow::timeOut_slot()
{
    calibrate->updatePclIcpTransferDebugWindow();

}

//void MainWindow::on_pushButton_testcalibrate_clicked()
//{
//    std::vector<PointPro> tcpPointList,cameraPointList;
//    std::vector<float> cameraTransferOut;
//    double maxErrorOut;

//    PointPro tmpPoint;
//    tmpPoint.positionValue.resize(6);
//    //0
//    tmpPoint.positionValue[0]=0;
//    tmpPoint.positionValue[1]=0;
//    tmpPoint.positionValue[2]=0;
//    tmpPoint.positionValue[3]=0;
//    tmpPoint.positionValue[4]=0;
//    tmpPoint.positionValue[5]=0;
//    tcpPointList.push_back(tmpPoint);
//    //1
//    tmpPoint.positionValue[0]=-10;
//    tmpPoint.positionValue[1]=0;
//    tmpPoint.positionValue[2]=0;
//    tmpPoint.positionValue[3]=0;
//    tmpPoint.positionValue[4]=0;
//    tmpPoint.positionValue[5]=0;
//    tcpPointList.push_back(tmpPoint);
//    //2
//    tmpPoint.positionValue[0]=10;
//    tmpPoint.positionValue[1]=20;
//    tmpPoint.positionValue[2]=0;
//    tmpPoint.positionValue[3]=0;
//    tmpPoint.positionValue[4]=0;
//    tmpPoint.positionValue[5]=0;
//    tcpPointList.push_back(tmpPoint);

//    //3
//    tmpPoint.positionValue[0]=30;
//    tmpPoint.positionValue[1]=-90;
//    tmpPoint.positionValue[2]=0;
//    tmpPoint.positionValue[3]=0;
//    tmpPoint.positionValue[4]=0;
//    tmpPoint.positionValue[5]=0;
//    tcpPointList.push_back(tmpPoint);

//    //4
//    tmpPoint.positionValue[0]=20;
//    tmpPoint.positionValue[1]=80;
//    tmpPoint.positionValue[2]=0;
//    tmpPoint.positionValue[3]=0;
//    tmpPoint.positionValue[4]=0;
//    tmpPoint.positionValue[5]=0;
//    tcpPointList.push_back(tmpPoint);

//    //not used
//    cameraPointList.push_back(tmpPoint);
//    //camera0
//    tmpPoint.positionValue[0]=110;
//    tmpPoint.positionValue[2]=200;
//    tmpPoint.positionValue[1]=0;
//    tmpPoint.positionValue[3]=0;
//    tmpPoint.positionValue[4]=0;
//    tmpPoint.positionValue[5]=0;
//    cameraPointList.push_back(tmpPoint);

//    //1
//    tmpPoint.positionValue[0]=90;
//    tmpPoint.positionValue[2]=180;
//    tmpPoint.positionValue[1]=0;
//    tmpPoint.positionValue[3]=0;
//    tmpPoint.positionValue[4]=0;
//    tmpPoint.positionValue[5]=0;
//    cameraPointList.push_back(tmpPoint);

//    //2
//    tmpPoint.positionValue[0]=70;
//    tmpPoint.positionValue[2]=290;
//    tmpPoint.positionValue[1]=0;
//    tmpPoint.positionValue[3]=0;
//    tmpPoint.positionValue[4]=0;
//    tmpPoint.positionValue[5]=0;
//    cameraPointList.push_back(tmpPoint);

//    //3
//    tmpPoint.positionValue[0]=80;
//    tmpPoint.positionValue[2]=-120;
//    tmpPoint.positionValue[1]=0;
//    tmpPoint.positionValue[3]=0;
//    tmpPoint.positionValue[4]=0;
//    tmpPoint.positionValue[5]=0;
//    cameraPointList.push_back(tmpPoint);

//    calibrate->calculateLaserCameraFrame(tcpPointList,cameraPointList,cameraTransferOut,maxErrorOut);
//}

void MainWindow::on_pushButton_testcalibrate_clicked()
{
    std::vector<PointPro> tcpPointList,cameraPointList;
    std::vector<float> cameraTransferOut;
    double maxErrorOut;

    PointPro tmpPoint;
    tmpPoint.positionValue.resize(6);
    //0
    tmpPoint.positionValue[0]=340;
    tmpPoint.positionValue[1]=341.3;
    tmpPoint.positionValue[2]=-3.3;
    tmpPoint.positionValue[3]=-170.7;
    tmpPoint.positionValue[4]=-4.65;
    tmpPoint.positionValue[5]=64.49;
    tcpPointList.push_back(tmpPoint);
    //1
    tmpPoint.positionValue[0]=365.9;
    tmpPoint.positionValue[1]=373.1;
    tmpPoint.positionValue[2]=5.49;
    tmpPoint.positionValue[3]=-170.7;
    tmpPoint.positionValue[4]=-4.65;
    tmpPoint.positionValue[5]=64.49;
    tcpPointList.push_back(tmpPoint);
    //2
    tmpPoint.positionValue[0]=365.37;
    tmpPoint.positionValue[1]=375.18;
    tmpPoint.positionValue[2]=62.59;
    tmpPoint.positionValue[3]=-170.7;
    tmpPoint.positionValue[4]=-4.65;
    tmpPoint.positionValue[5]=64.49;
    tcpPointList.push_back(tmpPoint);

    //3
    tmpPoint.positionValue[0]=309.37;
    tmpPoint.positionValue[1]=360.8;
    tmpPoint.positionValue[2]=65.07;
    tmpPoint.positionValue[3]=-170.7;
    tmpPoint.positionValue[4]=-4.65;
    tmpPoint.positionValue[5]=64.49;
    tcpPointList.push_back(tmpPoint);

    //4
    tmpPoint.positionValue[0]=340;
    tmpPoint.positionValue[1]=341.3;
    tmpPoint.positionValue[2]=-3.3;
    tmpPoint.positionValue[3]=-170.6;
    tmpPoint.positionValue[4]=-4.65;
    tmpPoint.positionValue[5]=64.49;
    tcpPointList.push_back(tmpPoint);

    //not used
    cameraPointList.push_back(tmpPoint);

    //1
    tmpPoint.positionValue[0]=-30.8;
    tmpPoint.positionValue[2]=0;
    tmpPoint.positionValue[1]=-261.5;
    tmpPoint.positionValue[3]=0;
    tmpPoint.positionValue[4]=0;
    tmpPoint.positionValue[5]=0;
    cameraPointList.push_back(tmpPoint);

    //2
    tmpPoint.positionValue[0]=-28.2;
    tmpPoint.positionValue[2]=0;
    tmpPoint.positionValue[1]=-317.5;
    tmpPoint.positionValue[3]=0;
    tmpPoint.positionValue[4]=0;
    tmpPoint.positionValue[5]=0;
    cameraPointList.push_back(tmpPoint);


    tmpPoint.positionValue[0]=30.5;
    tmpPoint.positionValue[2]=0;
    tmpPoint.positionValue[1]=-319.1;
    tmpPoint.positionValue[3]=0;
    tmpPoint.positionValue[4]=0;
    tmpPoint.positionValue[5]=0;
    cameraPointList.push_back(tmpPoint);


    //3
    tmpPoint.positionValue[0]=2.4;
    tmpPoint.positionValue[2]=0;
    tmpPoint.positionValue[1]=-252.5;//为啥变换顺序就完全不对了。
    tmpPoint.positionValue[3]=0;
    tmpPoint.positionValue[4]=0;
    tmpPoint.positionValue[5]=0;
    cameraPointList.push_back(tmpPoint);



    calibrate->calculateLaserCameraFrame(tcpPointList,cameraPointList,cameraTransferOut,maxErrorOut);
}


//void MainWindow::on_pushButton_testcalibrate_clicked()
//{
//    std::vector<PointPro> tcpPointList,cameraPointList;
//    std::vector<float> cameraTransferOut;
//    double maxErrorOut;

//    PointPro tmpPoint;
//    tmpPoint.positionValue.resize(6);
//    //0
//    tmpPoint.positionValue[0]=271.4;
//    tmpPoint.positionValue[1]=290.5;
//    tmpPoint.positionValue[2]=-6.3;
//    tmpPoint.positionValue[3]=-170.7;
//    tmpPoint.positionValue[4]=-3.6;
//    tmpPoint.positionValue[5]=67;
//    tcpPointList.push_back(tmpPoint);
//    //1
//    tmpPoint.positionValue[0]=291.2;
//    tmpPoint.positionValue[1]=323.7;
//    tmpPoint.positionValue[2]=-3;
//    tmpPoint.positionValue[3]=-169;
//    tmpPoint.positionValue[4]=-3.59;
//    tmpPoint.positionValue[5]=56.9;
//    tcpPointList.push_back(tmpPoint);
//    //2
//    tmpPoint.positionValue[0]=288.64;
//    tmpPoint.positionValue[1]=323.98;
//    tmpPoint.positionValue[2]=67.2;
//    tmpPoint.positionValue[3]=-169;
//    tmpPoint.positionValue[4]=-3.59;
//    tmpPoint.positionValue[5]=56.9;
//    tcpPointList.push_back(tmpPoint);

//    //3
//    tmpPoint.positionValue[0]=234.24;
//    tmpPoint.positionValue[1]=302.14;
//    tmpPoint.positionValue[2]=66.67;
//    tmpPoint.positionValue[3]=-169;
//    tmpPoint.positionValue[4]=-3.59;
//    tmpPoint.positionValue[5]=56.9;
//    tcpPointList.push_back(tmpPoint);

//    //4
//    tmpPoint.positionValue[0]=238.3;
//    tmpPoint.positionValue[1]=302.7;
//    tmpPoint.positionValue[2]=-1.23;
//    tmpPoint.positionValue[3]=-169;
//    tmpPoint.positionValue[4]=-3.59;
//    tmpPoint.positionValue[5]=56.9;
//    tcpPointList.push_back(tmpPoint);

//    //not used
//    cameraPointList.push_back(tmpPoint);
//    //camera0
//    tmpPoint.positionValue[0]=-32.49;
//    tmpPoint.positionValue[2]=0;
//    tmpPoint.positionValue[1]=-252.17;
//    tmpPoint.positionValue[3]=0;
//    tmpPoint.positionValue[4]=0;
//    tmpPoint.positionValue[5]=0;
//    cameraPointList.push_back(tmpPoint);

//    //1
//    tmpPoint.positionValue[0]=-31.31;
//    tmpPoint.positionValue[2]=0;
//    tmpPoint.positionValue[1]=-320.98;
//    tmpPoint.positionValue[3]=0;
//    tmpPoint.positionValue[4]=0;
//    tmpPoint.positionValue[5]=0;
//    cameraPointList.push_back(tmpPoint);

//    //2
//    tmpPoint.positionValue[0]=28.14;
//    tmpPoint.positionValue[2]=0;
//    tmpPoint.positionValue[1]=-321.64;
//    tmpPoint.positionValue[3]=0;
//    tmpPoint.positionValue[4]=0;
//    tmpPoint.positionValue[5]=0;
//    cameraPointList.push_back(tmpPoint);

//    //3
//    tmpPoint.positionValue[0]=25.35;
//    tmpPoint.positionValue[2]=0;
//    tmpPoint.positionValue[1]=-255.47;//为啥变换顺序就完全不对了。
//    tmpPoint.positionValue[3]=0;
//    tmpPoint.positionValue[4]=0;
//    tmpPoint.positionValue[5]=0;
//    cameraPointList.push_back(tmpPoint);



//    calibrate->calculateLaserCameraFrame(tcpPointList,cameraPointList,cameraTransferOut,maxErrorOut);
//}

//void MainWindow::on_pushButton_testcalibrate_clicked()
//{
//    std::vector<PointPro> tcpPointList,cameraPointList;
//    std::vector<float> cameraTransferOut;
//    double maxErrorOut;

//    PointPro tmpPoint;
//    tmpPoint.positionValue.resize(6);
//    //0
//    tmpPoint.positionValue[0]=293.4;
//    tmpPoint.positionValue[1]=300.9;
//    tmpPoint.positionValue[2]=-6.8;
//    tmpPoint.positionValue[3]=-178.8;
//    tmpPoint.positionValue[4]=-18.7;
//    tmpPoint.positionValue[5]=65.5;
//    tcpPointList.push_back(tmpPoint);
//    //1
//    tmpPoint.positionValue[0]=266.99;
//    tmpPoint.positionValue[1]=313.3;
//    tmpPoint.positionValue[2]=5.9;
//    tmpPoint.positionValue[3]=-178.7;
//    tmpPoint.positionValue[4]=-18.7;
//    tmpPoint.positionValue[5]=64.6;
//    tcpPointList.push_back(tmpPoint);
//    //2
//    tmpPoint.positionValue[0]=272.2;
//    tmpPoint.positionValue[1]=318.3;
//    tmpPoint.positionValue[2]=66;
//    tmpPoint.positionValue[3]=-178.7;
//    tmpPoint.positionValue[4]=-18.7;
//    tmpPoint.positionValue[5]=64.6;
//    tcpPointList.push_back(tmpPoint);

//    //3
//    tmpPoint.positionValue[0]=308.3;
//    tmpPoint.positionValue[1]=334.4;
//    tmpPoint.positionValue[2]=54.3;
//    tmpPoint.positionValue[3]=-178.7;
//    tmpPoint.positionValue[4]=-18.7;
//    tmpPoint.positionValue[5]=64.6;
//    tcpPointList.push_back(tmpPoint);

//    //4
//    tmpPoint.positionValue[0]=311.2;
//    tmpPoint.positionValue[1]=332.8;
//    tmpPoint.positionValue[2]=-4.4;
//    tmpPoint.positionValue[3]=-178.7;
//    tmpPoint.positionValue[4]=-18.7;
//    tmpPoint.positionValue[5]=64.6;
//    tcpPointList.push_back(tmpPoint);

//    //not used
//    cameraPointList.push_back(tmpPoint);
//    //camera0
//    tmpPoint.positionValue[0]=21.3;
//    tmpPoint.positionValue[2]=0;
//    tmpPoint.positionValue[1]=-261.9;
//    tmpPoint.positionValue[3]=0;
//    tmpPoint.positionValue[4]=0;
//    tmpPoint.positionValue[5]=0;
//    cameraPointList.push_back(tmpPoint);

//    //1
//    tmpPoint.positionValue[0]=19.5;
//    tmpPoint.positionValue[2]=0;
//    tmpPoint.positionValue[1]=-320.7;
//    tmpPoint.positionValue[3]=0;
//    tmpPoint.positionValue[4]=0;
//    tmpPoint.positionValue[5]=0;
//    cameraPointList.push_back(tmpPoint);

//    //2
//    tmpPoint.positionValue[0]=-21.4;
//    tmpPoint.positionValue[2]=0;
//    tmpPoint.positionValue[1]=-314.3;
//    tmpPoint.positionValue[3]=0;
//    tmpPoint.positionValue[4]=0;
//    tmpPoint.positionValue[5]=0;
//    cameraPointList.push_back(tmpPoint);

//    //3
//    tmpPoint.positionValue[0]=-28.7;
//    tmpPoint.positionValue[2]=0;
//    tmpPoint.positionValue[1]=-256.3;//为啥变换顺序就完全不对了。
//    tmpPoint.positionValue[3]=0;
//    tmpPoint.positionValue[4]=0;
//    tmpPoint.positionValue[5]=0;
//    cameraPointList.push_back(tmpPoint);



//    calibrate->calculateLaserCameraFrame(tcpPointList,cameraPointList,cameraTransferOut,maxErrorOut);
//}

void MainWindow::on_pushButton_showPoint_clicked()
{
    bool tmp1=ui->checkBox->isChecked();
    bool tmp2=ui->checkBox_2->isChecked();
    bool tmp3=ui->checkBox_3->isChecked();
    bool tmp4=ui->checkBox_4->isChecked();
    calibrate->showPclIcpTransferDebugWindow(tmp1,tmp2,tmp3,tmp4);
}

void MainWindow::on_pushButton_showPoint_2_clicked()
{

}

void MainWindow::on_pushButton_testPnp_clicked()
{
    vector<cv::Point3f> cornersPoint3d;
    vector<cv::Point2f> cornersPoint2d;
    Mat camMatrix;//内参
    Mat distCoeff;//畸变

    cornersPoint3d.push_back(cv::Point3f(0,0,0));
    cornersPoint3d.push_back(cv::Point3f(1,0,0));
    cornersPoint3d.push_back(cv::Point3f(0,2,0));
    cornersPoint3d.push_back(cv::Point3f(1,1,0));

    cornersPoint2d.push_back(cv::Point2f(10,0));
    cornersPoint2d.push_back(cv::Point2f(11,0));
    cornersPoint2d.push_back(cv::Point2f(10,2));
    cornersPoint2d.push_back(cv::Point2f(11,1));


     float fx = 1;
     float fy = 1;
     float u0 = 0;
     float v0 = 0;

     camMatrix = (Mat_<double>(3, 3) << fx, 0., u0, 0., fy, v0, 0., 0., 1.);
     distCoeff=(Mat_<double>(5, 1) << 0,0,0,0,0);

     Mat rotMat,transMat;
     solvePnP(cornersPoint3d, cornersPoint2d, camMatrix, distCoeff, rotMat, transMat);

     qDebug()<<"rotMat"<<rotMat.at<double>(0)<<rotMat.at<double>(1)<<rotMat.at<double>(2);
     qDebug()<<"transMat"<<transMat.at<double>(0)<<transMat.at<double>(1)<<transMat.at<double>(2);
     std::cout << "pnp rotMat : \n"  << rotMat << std::endl;
          std::cout << "pnp transMat : \n"  << transMat << std::endl;
     rotMat.convertTo(rotMat, CV_32F);    //旋转向量
     Rodrigues(rotMat, rotMat);
}

void MainWindow::on_pushButton_testTransfer_clicked()
{

    vector<cv::Point2f> cornersPoint_before;
    vector<cv::Point2f> cornersPoint_after;

    cornersPoint_before.push_back(cv::Point2f(0,0));
    cornersPoint_before.push_back(cv::Point2f(1,0));
    cornersPoint_before.push_back(cv::Point2f(0,2));
    cornersPoint_before.push_back(cv::Point2f(1,1));
//    cornersPoint_before.push_back(cv::Point2f(1,1));

    cornersPoint_after.push_back(cv::Point2f(100,10));
    cornersPoint_after.push_back(cv::Point2f(101,10));
    cornersPoint_after.push_back(cv::Point2f(100,12));
    cornersPoint_after.push_back(cv::Point2f(101,11));
//    cornersPoint_after.push_back(cv::Point2f(1,1));

     Mat pointEstimate;
     pointEstimate=estimateRigidTransform(cornersPoint_before, cornersPoint_after, 0);
//　　　warpMat = CvInvoke.EstimateRigidTransform(points_camera, points_robot, true);
//        Image<Gray, float> img = warpMat.ToImage<Gray, float>();
     qDebug()<<"transfer,,," <<pointEstimate.at<double>(0, 0)<<pointEstimate.at<double>(0, 1)
            <<pointEstimate.at<double>(0, 2)
             <<pointEstimate.at<double>(1, 0)<<pointEstimate.at<double>(1, 1)<<pointEstimate.at<double>(1, 2);
     std::cout << "pointEstimate : \n"  << pointEstimate << std::endl;
//         std::cout << "transEstimate : \n"  << transEstimate << std::endl;
//         std::cout << "rotaEstimate  : \n"  << rotaEstimate  << std::endl;


//         std::cout << "Test: before transform the center is " << refPose_x <<"  "<< refPose_y << std::endl;
//         refPose_x = transEstimate.at(0)* refPose_x + transEstimate.at(1)* refPose_y +
//     transEstimate.at(2);
//         refPose_y = transEstimate.at(3)* refPose_x + transEstimate.at(4)* refPose_y +
//     transEstimate.at(5);


//        public PointF TransformPoint(PointF pPoint)
//         {
//            //********************************************
//             // x = x'k*cost-y'k* sint+x0,
//            // y = x'k*sint+y'k* cost+y0.
//            //A = k*cost,B =-k* sint,C
//            //D = k*sint,E = k* cost,F
//            //*********************************************
//            PointF tPoint = new PointF();
//            tPoint.X = Convert.ToSingle(A * pPoint.X + B * pPoint.Y + C);
//            tPoint.Y = Convert.ToSingle(D * pPoint.X + E * pPoint.Y + F);
//            return tPoint;
//        }
}

void MainWindow::on_pushButton_testPnp_2_clicked()
{
    std::vector<PointPro> tcpPointList,cameraPointList;


    PointPro tmpPoint;
    tmpPoint.positionValue.resize(6);
    //0
    tmpPoint.positionValue[0]=0;
    tmpPoint.positionValue[1]=0;
    tmpPoint.positionValue[2]=0;
    tmpPoint.positionValue[3]=0;
    tmpPoint.positionValue[4]=0;
    tmpPoint.positionValue[5]=0;
    tcpPointList.push_back(tmpPoint);
    //1
    tmpPoint.positionValue[0]=1;
    tmpPoint.positionValue[1]=0;
    tmpPoint.positionValue[2]=0;
    tmpPoint.positionValue[3]=0;
    tmpPoint.positionValue[4]=0;
    tmpPoint.positionValue[5]=0;
    tcpPointList.push_back(tmpPoint);
    //2
    tmpPoint.positionValue[0]=1;
    tmpPoint.positionValue[1]=2;
    tmpPoint.positionValue[2]=0;
    tmpPoint.positionValue[3]=0;
    tmpPoint.positionValue[4]=0;
    tmpPoint.positionValue[5]=0;
    tcpPointList.push_back(tmpPoint);

    //3
    tmpPoint.positionValue[0]=1;
    tmpPoint.positionValue[1]=3;
    tmpPoint.positionValue[2]=0;
    tmpPoint.positionValue[3]=0;
    tmpPoint.positionValue[4]=0;
    tmpPoint.positionValue[5]=0;
    tcpPointList.push_back(tmpPoint);



    //camera0
    tmpPoint.positionValue[0]=10;
    tmpPoint.positionValue[2]=0;
    tmpPoint.positionValue[1]=0;
    tmpPoint.positionValue[3]=0;
    tmpPoint.positionValue[4]=0;
    tmpPoint.positionValue[5]=0;
    cameraPointList.push_back(tmpPoint);

    //1
    tmpPoint.positionValue[0]=11;
    tmpPoint.positionValue[2]=0;
    tmpPoint.positionValue[1]=0;
    tmpPoint.positionValue[3]=0;
    tmpPoint.positionValue[4]=0;
    tmpPoint.positionValue[5]=0;
    cameraPointList.push_back(tmpPoint);

    //2
    tmpPoint.positionValue[0]=11;
    tmpPoint.positionValue[2]=2;
    tmpPoint.positionValue[1]=0;
    tmpPoint.positionValue[3]=0;
    tmpPoint.positionValue[4]=0;
    tmpPoint.positionValue[5]=0;
    cameraPointList.push_back(tmpPoint);

    //3
    tmpPoint.positionValue[0]=11;
    tmpPoint.positionValue[2]=3;
    tmpPoint.positionValue[1]=0;
    tmpPoint.positionValue[3]=0;
    tmpPoint.positionValue[4]=0;
    tmpPoint.positionValue[5]=0;
    cameraPointList.push_back(tmpPoint);

    std::vector<float> cameraTransferOut;
    calibrate->calculateTransformMatrix_opencv(tcpPointList,cameraPointList,cameraTransferOut);
    qDebug()<<cameraTransferOut[0]<<cameraTransferOut[1]<<cameraTransferOut[2];
//     std::cout << "cameraTransferOut : \n"  << cameraTransferOut << std::endl;
}

void MainWindow::on_pushButton_test3pointtransmatrix_clicked()
{
    CalculateThreePointMatrixTransform tmpThree;
    vector<float> point1onA= {10,0,0};
    vector<float> point2onA= {-10,-20,0};
    vector<float> point3onA= {-30,90,0};

//    vector<float> point1onA= {-6.1896,6.2744,-2.6239};
//    vector<float> point2onA= {21.1387,-11.7097, -2.7459};
//    vector<float> point3onA= {11.1252,10.1226,-2.4942};

    //模拟绕Z轴转90度，再X轴平移50.
    //取三组点
    float NN = 100;
//    vector<float> point1onA= {NN, 0, 0};
//    vector<float> point2onA= {0, NN, 0};
//    vector<float> point3onA= {0, 0, NN};

    //3个点自己的x,y,z都需要有变化，否则为０，可能导致矩阵不可逆，无法求解．
//    std::vector<float> cameraTransfer= {10,5,60,10,20,-10};
    std::vector<float> cameraTransfer= {100,200,0,0,0,0};


    vector<float> point1onB= {50+0, 0, 0};
    vector<float> point2onB= {50 , 0, 0};
    vector<float> point3onB= {50+0, 0, 0};

    transferm(cameraTransfer,point1onA,point1onB);
    transferm(cameraTransfer,point2onA,point2onB);
    transferm(cameraTransfer,point3onA,point3onB);
//    point1onB[1]+=1;


    qDebug()<<" origin trans "<<cameraTransfer[0]<<cameraTransfer[1]<<cameraTransfer[2]
              <<cameraTransfer[3]<<cameraTransfer[4]<<cameraTransfer[5];


    Eigen::Matrix3f rotationOut;
    Eigen::Vector3f translationOut;
    float ratioOut;
//    vector<vector<float> > point1onAList,point1onBList;
    int tmpkey=tmpThree.calculateTransformNew2(point1onA, point2onA, point3onA,point1onB, point2onB, point3onB,
                                rotationOut,translationOut,ratioOut);
//    int tmpkey=tmpThree.calculateTransform(point1onA, point2onA, point3onA,point1onB, point2onB, point3onB,
//                                rotationOut,translationOut,ratioOut);
//    point1onAList.push_back(point1onA);
//    point1onAList.push_back(point2onA);
//    point1onAList.push_back(point3onA);
//    point1onBList.push_back(point1onB);
//    point1onBList.push_back(point2onB);
//    point1onBList.push_back(point3onB);
//    int tmpkey=tmpThree.calculateTransform2(point1onAList,point1onBList,
//                                rotationOut,translationOut,ratioOut);


    Rotation tmpRotation(rotationOut(0),rotationOut(1),rotationOut(2),
            rotationOut(3),rotationOut(4),rotationOut(5),
            rotationOut(6),rotationOut(7),rotationOut(8));
    double tmpw,tmpp,tmpr;
    tmpRotation.GetRPY(tmpw,tmpp,tmpr);

        qDebug()<<"result translationOut"<<translationOut[0]<<translationOut[1]<<translationOut[2]
              <<tmpw/M_PI*180<<tmpp/M_PI*180<<tmpr/M_PI*180;

//    KDL::Frame frame_trans = Frame(tmpRotation,
//                    Vector (translationOut[0],
//                        translationOut[1],
//                        translationOut[2]));
//    KDL::Frame frame_inv=frame_trans.Inverse();
//    frame_inv.M.GetRPY(tmpw,tmpp,tmpr);

//    qDebug()<<"result inverse translationOut"<<frame_inv.p[0]<<frame_inv.p[1]<<frame_inv.p[2]
//          <<tmpw/M_PI*180<<tmpp/M_PI*180<<tmpr/M_PI*180;

        //test

        Rotation M;
        KDL::Frame frame_trans = Frame(M.RPY(cameraTransfer[3]*M_PI/180.0,
                        cameraTransfer[4]*M_PI/180.0,
                        cameraTransfer[5]*M_PI/180.0),
                        Vector (cameraTransfer[0],
                            cameraTransfer[1],
                            cameraTransfer[2]));
            KDL::Frame frame_inv=frame_trans.Inverse();
            frame_inv.M.GetRPY(tmpw,tmpp,tmpr);

            qDebug()<<"result inverse translationOut"<<frame_inv.p[0]<<frame_inv.p[1]<<frame_inv.p[2]
                  <<tmpw/M_PI*180<<tmpp/M_PI*180<<tmpr/M_PI*180;



    std::vector<float> cameraTransfer2= {frame_inv.p[0],frame_inv.p[1],frame_inv.p[2],
                                         tmpw/M_PI*180,tmpp/M_PI*180,tmpr/M_PI*180};

//    std::vector<float> cameraTransfer2= {translationOut[0],translationOut[1],translationOut[2],
//                                         tmpw/M_PI*180,tmpp/M_PI*180,tmpr/M_PI*180};

    vector<float> point1onC= {0, 0, 0};
    vector<float> point2onC= {0, 0, 0};
    vector<float> point3onC= {0, 0, 0};

    transferm(cameraTransfer2,point1onB,point1onC);
    transferm(cameraTransfer2,point2onB,point2onC);
    transferm(cameraTransfer2,point3onB,point3onC);

    qDebug()<<"result point a"<<point1onA[0]<<point1onA[1]<<point1onA[2]<<" b"
              <<point1onB[0]<<point1onB[1]<<point1onB[2]<<" c"
                <<point1onC[0]<<point1onC[1]<<point1onC[2];


}

void MainWindow::transferm(std::vector<float> cameraTransfer,vector<float> pointIn,vector<float> &pointOut)
{
    Rotation M;

    KDL::Frame frame_trans = Frame(M.RPY(cameraTransfer[3]*M_PI/180.0,
                    cameraTransfer[4]*M_PI/180.0,
                    cameraTransfer[5]*M_PI/180.0),
                    Vector (cameraTransfer[0],
                        cameraTransfer[1],
                        cameraTransfer[2]));

    KDL::Vector tmpVector2(pointIn[0],pointIn[1],pointIn[2]);
    KDL::Frame frame1(tmpVector2);

//    KDL::Frame frame2=frame1*frame_trans;
//    KDL::Frame frame2=frame1*(frame_trans.Inverse());
//    KDL::Frame frame2=frame_trans.Inverse()*frame1;
     KDL::Frame frame2=frame_trans*frame1;
    pointOut[0]=frame2.p[0];
    pointOut[1]=frame2.p[1];
    pointOut[2]=frame2.p[2];

}
